
#include "dynamixel.h"
#include "dynamixel_registers.h"

Dynamixel::Dynamixel(void)
{
  servo_id = 1;
}

void Dynamixel::work(uint32_t time)
{
  dynamixel_hal.work(time);
}

void Dynamixel::setTorqueEnable(bool on)
{
  dynamixel_hal.writeWord(servo_id, DynamixelRX28::P_CW_ANGLE_LIMIT_L, 0);
  dynamixel_hal.writeWord(servo_id, DynamixelRX28::P_CCW_ANGLE_LIMIT_L, 0);
  //dynamixel_hal.writeWord(servo_id, DynamixelRX28::P_GOAL_SPEED_L, 0x3ff + 1024);
  dynamixel_hal.writeByte(servo_id, DynamixelRX28::P_TORQUE_ENABLE, on ? 1 : 0);
}

void Dynamixel::setSpeed(int speed)
{
  if (speed < 0) {
    speed = -speed;
    speed %= 1024;
    speed += 1024;
  } else {
    speed %= 1024;
  }
  dynamixel_hal.writeWord(servo_id, DynamixelRX28::P_GOAL_SPEED_L, speed);
}

void Dynamixel::setLed(bool on)
{
  dynamixel_hal.writeByte(servo_id, DynamixelRX28::P_LED, on?1:0);
}

int Dynamixel::getTemperature(void)
{
  int temperature = 0;
  dynamixel_hal.readByte(servo_id, DynamixelRX28::P_PRESENT_TEMPERATURE, &temperature);
  return temperature;
}

int Dynamixel::getVoltage(void)
{
  int voltage = 0;
  dynamixel_hal.readByte(servo_id, DynamixelRX28::P_PRESENT_VOLTAGE, &voltage);
  return voltage;
}
